#include "base_interfase/action/nav.hpp"
#include "geometry_msgs/msg/twist.hpp"
#include "turtlesim_msgs/msg/pose.hpp"
#include <memory>
#include <rclcpp/rclcpp.hpp>
#include <rclcpp_action/rclcpp_action.hpp>
#include <thread>

using base_interfase::action::Nav;

class exec04ActionServer : public rclcpp::Node {
public:
  exec04ActionServer() : Node("exer04_action_server") {

    // 发布
    publisher_ = this->create_publisher<geometry_msgs::msg::Twist>(
        "turtle1/cmd_vel", 10);

    // 订阅
    subscription_ = this->create_subscription<turtlesim_msgs::msg::Pose>(
        "turtle1/pose", 10,
        std::bind(&exec04ActionServer::topic_callback, this,
                  std::placeholders::_1));
    // Create an action server with a callback function to handle goal requests
    action_server_ = rclcpp_action::create_server<Nav>(
        this, "nav",
        std::bind(&exec04ActionServer ::handle_goal, this,
                  std::placeholders::_1, std::placeholders::_2),
        std::bind(&exec04ActionServer ::handle_cancel, this,
                  std::placeholders::_1),
        std::bind(&exec04ActionServer::handle_accepted, this,
                  std::placeholders::_1));
  }

private:
  rclcpp_action::Server<Nav>::SharedPtr action_server_;

  // 发布
  rclcpp::Publisher<geometry_msgs::msg::Twist>::SharedPtr publisher_;

  // 订阅
  rclcpp::Subscription<turtlesim_msgs::msg::Pose>::SharedPtr subscription_;

  rclcpp_action::GoalResponse
  handle_goal(const rclcpp_action::GoalUUID &uuid,
              std::shared_ptr<const Nav::Goal> goal) {
    RCLCPP_INFO(this->get_logger(), "Received goal request with order %.2f",
                goal.get()->theta_goal);
    (void)uuid;
    // Accept all goals
    return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE;
  }

  rclcpp_action::CancelResponse handle_cancel(
      const std::shared_ptr<rclcpp_action::ServerGoalHandle<Nav>> goal_handle) {
    RCLCPP_INFO(this->get_logger(), "Received request to cancel goal");
    (void)goal_handle;
    // Accept all cancel requests
    return rclcpp_action::CancelResponse::ACCEPT;
  }

  float x, y, theta;

  void topic_callback(const turtlesim_msgs::msg::Pose::SharedPtr msg) {
    x = msg->x;
    y = msg->y;
    theta = msg->theta;
    // RCLCPP_INFO(this->get_logger(), "Turtle Pose: [%.2f, %.2f, %.2f]",
    // msg->x,
    //             msg->y, msg->theta);
  }

  void handle_accepted(
      const std::shared_ptr<rclcpp_action::ServerGoalHandle<Nav>> goal_handle) {
    // This needs to return quickly to avoid blocking the executor, so spin up a
    // new thread
    std::thread{
        std::bind(&exec04ActionServer::execute, this, std::placeholders::_1),
        goal_handle}
        .detach();
  }

  void execute(
      const std::shared_ptr<rclcpp_action::ServerGoalHandle<Nav>> goal_handle) {
    RCLCPP_INFO(this->get_logger(), "Executing goal");
    rclcpp::Rate loop_rate(1);
    const auto goal = goal_handle->get_goal();
    auto feedback = std::make_shared<Nav::Feedback>();
    auto result = std::make_shared<Nav::Result>();

    // 模拟一个长时间运行的任务
    while (rclcpp::ok()) {

      // 检查是否有取消请求
      if (goal_handle->is_canceling()) {
        goal_handle->canceled(result);
        RCLCPP_INFO(this->get_logger(), "Goal canceled");
        return;
      }

      geometry_msgs::msg::Twist twist;

      // 计算角度差
      float angle_to_goal = atan2(goal->y_goal - y, goal->x_goal - x);
      float angle_diff = angle_to_goal - theta;
      
      // 归一化角度差到[-pi, pi]
      while (angle_diff > M_PI)
        angle_diff -= 2 * M_PI;
      while (angle_diff < -M_PI)
        angle_diff += 2 * M_PI;
      // 设置线速度和角速度
      twist.linear.x = 1.0;               // 固定线速度
      twist.angular.z = 4.0 * angle_diff; // 比例控制角速度

      // 发布速度命令
      publisher_->publish(twist);

      // 计算距离
      float distance =
          sqrt(pow((goal->x_goal - x), 2) + pow((goal->y_goal - y), 2));

      RCLCPP_INFO(this->get_logger(), "距离目标还有: %.2f", distance);

      if (distance < 0.1) {
        break;
      }
      // 更新反馈
      feedback->distance = distance;
      // 连续反馈
      goal_handle->publish_feedback(feedback);

      loop_rate.sleep();
    }

    // 返回最终结果
    if (rclcpp::ok()) {
      result->x_turtle = goal->x_goal;

      goal_handle->succeed(result);
      RCLCPP_INFO(this->get_logger(), "目标成功完成");
    }
  }
};

int main(int argc, char *argv[]) {
  rclcpp::init(argc, argv);
  rclcpp::spin(std::make_shared<exec04ActionServer>());
  rclcpp::shutdown();
  return 0;
}
